#include <stdio.h>
#include <math.h>
#include "Parking3.h"

/***********************************************/
/* Common functions for defuzzification methods*/
/***********************************************/

static double center(MembershipFunction mf) {
 return mf.center(mf);
}

/***********************************************/
/*  Common functions to create fuzzy numbers   */
/***********************************************/

static double MF_xfl_singleton_equal(MembershipFunction _mf,double x);

static FuzzyNumber createCrispNumber(double value) {
 FuzzyNumber fn;
 fn.crisp = 1;
 fn.crispvalue = value;
 fn.length = 0;
 fn.inputlength = 0;
 return fn;
}

static FuzzyNumber createFuzzyNumber(int length, int inputlength,
                                     double*input, OperatorSet op) {
 int i;
 FuzzyNumber fn;
 fn.crisp = 0;
 fn.crispvalue = 0;
 fn.length = length;
 for(i=0; i<length; i++) fn.degree[i] = 0;
 fn.inputlength = inputlength;
 fn.op = op;
 return fn;
}

/***********************************************/
/*  Functions to compute single propositions   */
/***********************************************/

static double _isEqual(MembershipFunction mf, FuzzyNumber fn) {
 return mf.compute_eq(mf,fn.crispvalue);
}

/***************************************/
/*  MembershipFunction MF_xfl_triangle  */
/***************************************/

static double MF_xfl_triangle_equal(MembershipFunction _mf,double x) {
 double a = _mf.param[0];
 double b = _mf.param[1];
 double c = _mf.param[2];
 return ( (a-x)<0 && (x-b)<=0? (x-a)/(b-a) : ((b-x)<0 && (x-c)<0? (c-x)/(c-b) : 0)); 
}

static MembershipFunction createMF_xfl_triangle( double min, double max, double step, double *param,int length) {
 int i;
 MembershipFunction _mf;
 _mf.min = min;
 _mf.max = max;
 _mf.step = step;
 for(i=0;i<length;i++) _mf.param[i] = param[i];
 _mf.compute_eq = MF_xfl_triangle_equal;
 _mf.compute_greq = NULL;
 _mf.compute_smeq = NULL;
 _mf.center = NULL;
 _mf.basis = NULL;
 return _mf;
}

/***************************************/
/*  MembershipFunction MF_xfl_singleton  */
/***************************************/

static double MF_xfl_singleton_center(MembershipFunction _mf) {
 double a = _mf.param[0];
  return a; 
}

static MembershipFunction createMF_xfl_singleton( double min, double max, double step, double *param,int length) {
 int i;
 MembershipFunction _mf;
 _mf.min = min;
 _mf.max = max;
 _mf.step = step;
 for(i=0;i<length;i++) _mf.param[i] = param[i];
 _mf.compute_eq = NULL;
 _mf.compute_greq = NULL;
 _mf.compute_smeq = NULL;
 _mf.center = MF_xfl_singleton_center;
 _mf.basis = NULL;
 return _mf;
}

/***************************************/
/*  Operatorset OP_opset */
/***************************************/

static double OP_opset_And(double a, double b) {
  return a*b; 
}

static double OP_opset_Defuz(FuzzyNumber mf) {
 double min = mf.conc[0].min;
 double max = mf.conc[0].max;
   double num=0, denom=0;
   int i;
   for(i=0; i<mf.length; i++) {
    num += mf.degree[i] * center(mf.conc[i]);
    denom += mf.degree[i];
   }
   if(denom==0) return (min+max)/2;
   return num/denom;
}

static OperatorSet createOP_opset() {
 OperatorSet op;
 op.and = OP_opset_And;
 op.or = NULL;
 op.also = NULL;
 op.imp = NULL;
 op.not = NULL;
 op.very = NULL;
 op.moreorless = NULL;
 op.slightly = NULL;
 op.defuz = OP_opset_Defuz;
 return op;
}

/***************************************/
/*  Operatorset OP_classifset */
/***************************************/

static double OP_classifset_And(double a, double b) {
  return a*b; 
}

static double OP_classifset_Defuz(FuzzyNumber mf) {
   double mx=0, c=0;
   int i;
   for(i=0; i<mf.length; i++)
    if(mf.degree[i] >= mx) { c = center(mf.conc[i]); mx = mf.degree[i]; }
   return c;
}

static OperatorSet createOP_classifset() {
 OperatorSet op;
 op.and = OP_classifset_And;
 op.or = NULL;
 op.also = NULL;
 op.imp = NULL;
 op.not = NULL;
 op.very = NULL;
 op.moreorless = NULL;
 op.slightly = NULL;
 op.defuz = OP_classifset_Defuz;
 return op;
}


/***************************************/
/*  Type TP_Tx */
/***************************************/

static TP_Tx createTP_Tx() {
 TP_Tx tp;
 double min = -10.0;
 double max = 10.0;
 double step = 0.0784313725490196;
 double _p_mf0[3] = { -13.3,-10.0,-5.0 };
 double _p_mf1[3] = { -10.0,-5.0,-2.2 };
 double _p_mf2[3] = { -5.0,-2.2,0.0 };
 double _p_mf3[3] = { -2.2,0.0,2.2 };
 double _p_mf4[3] = { 0.0,2.2,5.0 };
 double _p_mf5[3] = { 2.2,5.0,10.0 };
 double _p_mf6[3] = { 5.0,10.0,13.3 };
 tp.mf0 = createMF_xfl_triangle(min,max,step,_p_mf0,3);
 tp.mf1 = createMF_xfl_triangle(min,max,step,_p_mf1,3);
 tp.mf2 = createMF_xfl_triangle(min,max,step,_p_mf2,3);
 tp.mf3 = createMF_xfl_triangle(min,max,step,_p_mf3,3);
 tp.mf4 = createMF_xfl_triangle(min,max,step,_p_mf4,3);
 tp.mf5 = createMF_xfl_triangle(min,max,step,_p_mf5,3);
 tp.mf6 = createMF_xfl_triangle(min,max,step,_p_mf6,3);
 return tp;
}

/***************************************/
/*  Type TP_Tangle */
/***************************************/

static TP_Tangle createTP_Tangle() {
 TP_Tangle tp;
 double min = -180.0;
 double max = 180.0;
 double step = 1.411764705882353;
 double _p_mf0[3] = { -240.0,-180.0,-130.0 };
 double _p_mf1[3] = { -180.0,-130.0,-50.0 };
 double _p_mf2[3] = { -130.0,-50.0,0.0 };
 double _p_mf3[3] = { -50.0,0.0,50.0 };
 double _p_mf4[3] = { 0.0,50.0,130.0 };
 double _p_mf5[3] = { 50.0,130.0,180.0 };
 double _p_mf6[3] = { 130.0,180.0,240.0 };
 tp.mf0 = createMF_xfl_triangle(min,max,step,_p_mf0,3);
 tp.mf1 = createMF_xfl_triangle(min,max,step,_p_mf1,3);
 tp.mf2 = createMF_xfl_triangle(min,max,step,_p_mf2,3);
 tp.mf3 = createMF_xfl_triangle(min,max,step,_p_mf3,3);
 tp.mf4 = createMF_xfl_triangle(min,max,step,_p_mf4,3);
 tp.mf5 = createMF_xfl_triangle(min,max,step,_p_mf5,3);
 tp.mf6 = createMF_xfl_triangle(min,max,step,_p_mf6,3);
 return tp;
}

/***************************************/
/*  Type TP_TY */
/***************************************/

static TP_TY createTP_TY() {
 TP_TY tp;
 double min = -2.0;
 double max = 30.0;
 double step = 0.12549019607843137;
 double _p_mfTG[3] = { -4.0,-2.0,0.0 };
 double _p_mfCL[3] = { -2.0,0.0,2.5 };
 double _p_mfNE[3] = { 0.0,2.5,5.0 };
 double _p_mfMI[3] = { 2.5,5.0,30.0 };
 double _p_mfFA[3] = { 5.0,30.0,35.0 };
 tp.mfTG = createMF_xfl_triangle(min,max,step,_p_mfTG,3);
 tp.mfCL = createMF_xfl_triangle(min,max,step,_p_mfCL,3);
 tp.mfNE = createMF_xfl_triangle(min,max,step,_p_mfNE,3);
 tp.mfMI = createMF_xfl_triangle(min,max,step,_p_mfMI,3);
 tp.mfFA = createMF_xfl_triangle(min,max,step,_p_mfFA,3);
 return tp;
}

/***************************************/
/*  Type TP_TX */
/***************************************/

static TP_TX createTP_TX() {
 TP_TX tp;
 double min = -10.0;
 double max = 10.0;
 double step = 0.0784313725490196;
 double _p_mfLB[3] = { -13.3,-10.0,-2.0 };
 double _p_mfLS[3] = { -10.0,-2.0,0.0 };
 double _p_mfCE[3] = { -2.0,0.0,2.0 };
 double _p_mfRS[3] = { 0.0,2.0,10.0 };
 double _p_mfRB[3] = { 2.0,10.0,13.3 };
 tp.mfLB = createMF_xfl_triangle(min,max,step,_p_mfLB,3);
 tp.mfLS = createMF_xfl_triangle(min,max,step,_p_mfLS,3);
 tp.mfCE = createMF_xfl_triangle(min,max,step,_p_mfCE,3);
 tp.mfRS = createMF_xfl_triangle(min,max,step,_p_mfRS,3);
 tp.mfRB = createMF_xfl_triangle(min,max,step,_p_mfRB,3);
 return tp;
}

/***************************************/
/*  Type TP_TAngle */
/***************************************/

static TP_TAngle createTP_TAngle() {
 TP_TAngle tp;
 double min = -180.0;
 double max = 180.0;
 double step = 1.411764705882353;
 double _p_mfLB[3] = { -240.0,-180.0,-30.0 };
 double _p_mfLE[3] = { -180.0,-30.0,0.0 };
 double _p_mfCE[3] = { -30.0,0.0,30.0 };
 double _p_mfRI[3] = { 0.0,30.0,180.0 };
 double _p_mfRB[3] = { 30.0,180.0,240.0 };
 tp.mfLB = createMF_xfl_triangle(min,max,step,_p_mfLB,3);
 tp.mfLE = createMF_xfl_triangle(min,max,step,_p_mfLE,3);
 tp.mfCE = createMF_xfl_triangle(min,max,step,_p_mfCE,3);
 tp.mfRI = createMF_xfl_triangle(min,max,step,_p_mfRI,3);
 tp.mfRB = createMF_xfl_triangle(min,max,step,_p_mfRB,3);
 return tp;
}

/***************************************/
/*  Type TP_TPosition */
/***************************************/

static TP_TPosition createTP_TPosition() {
 TP_TPosition tp;
 double min = 0.0;
 double max = 4.0;
 double step = 0.01568627450980392;
 double _p_mfFC[1] = { 0.0 };
 double _p_mfFU[1] = { 1.0 };
 double _p_mfNC[1] = { 2.0 };
 double _p_mfNU[1] = { 3.0 };
 double _p_mfCC[1] = { 4.0 };
 tp.mfFC = createMF_xfl_singleton(min,max,step,_p_mfFC,1);
 tp.mfFU = createMF_xfl_singleton(min,max,step,_p_mfFU,1);
 tp.mfNC = createMF_xfl_singleton(min,max,step,_p_mfNC,1);
 tp.mfNU = createMF_xfl_singleton(min,max,step,_p_mfNU,1);
 tp.mfCC = createMF_xfl_singleton(min,max,step,_p_mfCC,1);
 return tp;
}

/***************************************/
/*  Type TP_TWay */
/***************************************/

static TP_TWay createTP_TWay() {
 TP_TWay tp;
 double min = -2.0;
 double max = 2.0;
 double step = 0.01568627450980392;
 double _p_mfPB[1] = { -2.0 };
 double _p_mfBA[1] = { -1.0 };
 double _p_mfST[1] = { 0.0 };
 double _p_mfFO[1] = { 1.0 };
 double _p_mfPR[1] = { 2.0 };
 tp.mfPB = createMF_xfl_singleton(min,max,step,_p_mfPB,1);
 tp.mfBA = createMF_xfl_singleton(min,max,step,_p_mfBA,1);
 tp.mfST = createMF_xfl_singleton(min,max,step,_p_mfST,1);
 tp.mfFO = createMF_xfl_singleton(min,max,step,_p_mfFO,1);
 tp.mfPR = createMF_xfl_singleton(min,max,step,_p_mfPR,1);
 return tp;
}

/***************************************/
/*  Type TP_TPlan */
/***************************************/

static TP_TPlan createTP_TPlan() {
 TP_TPlan tp;
 double min = -2.0;
 double max = 2.0;
 double step = 0.01568627450980392;
 double _p_mfPB[3] = { -3.0,-2.0,-1.0 };
 double _p_mfBA[3] = { -2.0,-1.0,0.0 };
 double _p_mfST[3] = { -1.0,0.0,1.0 };
 double _p_mfFO[3] = { 0.0,1.0,2.0 };
 double _p_mfPR[3] = { 1.0,2.0,3.0 };
 tp.mfPB = createMF_xfl_triangle(min,max,step,_p_mfPB,3);
 tp.mfBA = createMF_xfl_triangle(min,max,step,_p_mfBA,3);
 tp.mfST = createMF_xfl_triangle(min,max,step,_p_mfST,3);
 tp.mfFO = createMF_xfl_triangle(min,max,step,_p_mfFO,3);
 tp.mfPR = createMF_xfl_triangle(min,max,step,_p_mfPR,3);
 return tp;
}

/***************************************/
/*  Type TP_TGamma */
/***************************************/

static TP_TGamma createTP_TGamma() {
 TP_TGamma tp;
 double min = -0.4;
 double max = 0.4;
 double step = 0.0031372549019607846;
 double _p_mfNB[3] = { -0.5,-0.4,-0.2 };
 double _p_mfNS[3] = { -0.4,-0.2,-0.0 };
 double _p_mfZE[3] = { -0.2,0.0,0.2 };
 double _p_mfPS[3] = { 0.0,0.2,0.4 };
 double _p_mfPB[3] = { 0.2,0.4,0.5 };
 tp.mfNB = createMF_xfl_triangle(min,max,step,_p_mfNB,3);
 tp.mfNS = createMF_xfl_triangle(min,max,step,_p_mfNS,3);
 tp.mfZE = createMF_xfl_triangle(min,max,step,_p_mfZE,3);
 tp.mfPS = createMF_xfl_triangle(min,max,step,_p_mfPS,3);
 tp.mfPB = createMF_xfl_triangle(min,max,step,_p_mfPB,3);
 return tp;
}

/***************************************/
/*  Type TP_TWheel */
/***************************************/

static TP_TWheel createTP_TWheel() {
 TP_TWheel tp;
 double min = -0.4;
 double max = 0.4;
 double step = 0.0031372549019607846;
 double _p_mf0[1] = { -0.4 };
 double _p_mf1[1] = { 0.4 };
 double _p_mf2[1] = { 0.0 };
 tp.mf0 = createMF_xfl_singleton(min,max,step,_p_mf0,1);
 tp.mf1 = createMF_xfl_singleton(min,max,step,_p_mf1,1);
 tp.mf2 = createMF_xfl_singleton(min,max,step,_p_mf2,1);
 return tp;
}

/***************************************/
/*  Type TP_TCelerity */
/***************************************/

static TP_TCelerity createTP_TCelerity() {
 TP_TCelerity tp;
 double min = 0.0;
 double max = 1.0;
 double step = 0.00392156862745098;
 double _p_stop[1] = { 0.0 };
 double _p_slow[1] = { 0.25 };
 double _p_middle[1] = { 0.5 };
 double _p_quick[1] = { 0.75 };
 double _p_fast[1] = { 1.0 };
 tp.stop = createMF_xfl_singleton(min,max,step,_p_stop,1);
 tp.slow = createMF_xfl_singleton(min,max,step,_p_slow,1);
 tp.middle = createMF_xfl_singleton(min,max,step,_p_middle,1);
 tp.quick = createMF_xfl_singleton(min,max,step,_p_quick,1);
 tp.fast = createMF_xfl_singleton(min,max,step,_p_fast,1);
 return tp;
}

/***************************************/
/*  Rulebase RL_position */
/***************************************/

static void RL_position(FuzzyNumber x, FuzzyNumber y, FuzzyNumber *position) {
 OperatorSet _op = createOP_classifset();
 double _rl, _output;
 int _i_position=0;
 TP_TX _t_x = createTP_TX();
 TP_TY _t_y = createTP_TY();
 TP_TPosition _t_position = createTP_TPosition();
 double _input[2];
 _input[0] = x.crispvalue;
 _input[1] = y.crispvalue;
 *position = createFuzzyNumber(25,2,_input,_op);
 _rl = _op.and(_isEqual(_t_x.mfLB,x),_isEqual(_t_y.mfTG,y));
 position->degree[_i_position] = _rl;
 position->conc[_i_position] =  _t_position.mfNU;
 _i_position++;
 _rl = _op.and(_isEqual(_t_x.mfLB,x),_isEqual(_t_y.mfCL,y));
 position->degree[_i_position] = _rl;
 position->conc[_i_position] =  _t_position.mfNU;
 _i_position++;
 _rl = _op.and(_isEqual(_t_x.mfLB,x),_isEqual(_t_y.mfNE,y));
 position->degree[_i_position] = _rl;
 position->conc[_i_position] =  _t_position.mfNU;
 _i_position++;
 _rl = _op.and(_isEqual(_t_x.mfLB,x),_isEqual(_t_y.mfMI,y));
 position->degree[_i_position] = _rl;
 position->conc[_i_position] =  _t_position.mfFU;
 _i_position++;
 _rl = _op.and(_isEqual(_t_x.mfLB,x),_isEqual(_t_y.mfFA,y));
 position->degree[_i_position] = _rl;
 position->conc[_i_position] =  _t_position.mfFU;
 _i_position++;
 _rl = _op.and(_isEqual(_t_x.mfLS,x),_isEqual(_t_y.mfTG,y));
 position->degree[_i_position] = _rl;
 position->conc[_i_position] =  _t_position.mfNU;
 _i_position++;
 _rl = _op.and(_isEqual(_t_x.mfLS,x),_isEqual(_t_y.mfCL,y));
 position->degree[_i_position] = _rl;
 position->conc[_i_position] =  _t_position.mfNU;
 _i_position++;
 _rl = _op.and(_isEqual(_t_x.mfLS,x),_isEqual(_t_y.mfNE,y));
 position->degree[_i_position] = _rl;
 position->conc[_i_position] =  _t_position.mfNU;
 _i_position++;
 _rl = _op.and(_isEqual(_t_x.mfLS,x),_isEqual(_t_y.mfMI,y));
 position->degree[_i_position] = _rl;
 position->conc[_i_position] =  _t_position.mfFU;
 _i_position++;
 _rl = _op.and(_isEqual(_t_x.mfLS,x),_isEqual(_t_y.mfFA,y));
 position->degree[_i_position] = _rl;
 position->conc[_i_position] =  _t_position.mfFU;
 _i_position++;
 _rl = _op.and(_isEqual(_t_x.mfCE,x),_isEqual(_t_y.mfTG,y));
 position->degree[_i_position] = _rl;
 position->conc[_i_position] =  _t_position.mfCC;
 _i_position++;
 _rl = _op.and(_isEqual(_t_x.mfCE,x),_isEqual(_t_y.mfCL,y));
 position->degree[_i_position] = _rl;
 position->conc[_i_position] =  _t_position.mfCC;
 _i_position++;
 _rl = _op.and(_isEqual(_t_x.mfCE,x),_isEqual(_t_y.mfNE,y));
 position->degree[_i_position] = _rl;
 position->conc[_i_position] =  _t_position.mfNC;
 _i_position++;
 _rl = _op.and(_isEqual(_t_x.mfCE,x),_isEqual(_t_y.mfMI,y));
 position->degree[_i_position] = _rl;
 position->conc[_i_position] =  _t_position.mfFC;
 _i_position++;
 _rl = _op.and(_isEqual(_t_x.mfCE,x),_isEqual(_t_y.mfFA,y));
 position->degree[_i_position] = _rl;
 position->conc[_i_position] =  _t_position.mfFC;
 _i_position++;
 _rl = _op.and(_isEqual(_t_x.mfRS,x),_isEqual(_t_y.mfTG,y));
 position->degree[_i_position] = _rl;
 position->conc[_i_position] =  _t_position.mfNU;
 _i_position++;
 _rl = _op.and(_isEqual(_t_x.mfRS,x),_isEqual(_t_y.mfCL,y));
 position->degree[_i_position] = _rl;
 position->conc[_i_position] =  _t_position.mfNU;
 _i_position++;
 _rl = _op.and(_isEqual(_t_x.mfRS,x),_isEqual(_t_y.mfNE,y));
 position->degree[_i_position] = _rl;
 position->conc[_i_position] =  _t_position.mfNU;
 _i_position++;
 _rl = _op.and(_isEqual(_t_x.mfRS,x),_isEqual(_t_y.mfMI,y));
 position->degree[_i_position] = _rl;
 position->conc[_i_position] =  _t_position.mfFU;
 _i_position++;
 _rl = _op.and(_isEqual(_t_x.mfRS,x),_isEqual(_t_y.mfFA,y));
 position->degree[_i_position] = _rl;
 position->conc[_i_position] =  _t_position.mfFU;
 _i_position++;
 _rl = _op.and(_isEqual(_t_x.mfRB,x),_isEqual(_t_y.mfTG,y));
 position->degree[_i_position] = _rl;
 position->conc[_i_position] =  _t_position.mfNU;
 _i_position++;
 _rl = _op.and(_isEqual(_t_x.mfRB,x),_isEqual(_t_y.mfCL,y));
 position->degree[_i_position] = _rl;
 position->conc[_i_position] =  _t_position.mfNU;
 _i_position++;
 _rl = _op.and(_isEqual(_t_x.mfRB,x),_isEqual(_t_y.mfNE,y));
 position->degree[_i_position] = _rl;
 position->conc[_i_position] =  _t_position.mfNU;
 _i_position++;
 _rl = _op.and(_isEqual(_t_x.mfRB,x),_isEqual(_t_y.mfMI,y));
 position->degree[_i_position] = _rl;
 position->conc[_i_position] =  _t_position.mfFU;
 _i_position++;
 _rl = _op.and(_isEqual(_t_x.mfRB,x),_isEqual(_t_y.mfFA,y));
 position->degree[_i_position] = _rl;
 position->conc[_i_position] =  _t_position.mfFU;
 _i_position++;
 _output = _op.defuz(*position);
 *position = createCrispNumber(_output);
}

/***************************************/
/*  Rulebase RL_planning */
/***************************************/

static void RL_planning(FuzzyNumber pos, FuzzyNumber angle, FuzzyNumber *plan) {
 OperatorSet _op = createOP_classifset();
 double _rl, _output;
 int _i_plan=0;
 TP_TPosition _t_pos = createTP_TPosition();
 TP_TAngle _t_angle = createTP_TAngle();
 TP_TWay _t_plan = createTP_TWay();
 double _input[2];
 _input[0] = pos.crispvalue;
 _input[1] = angle.crispvalue;
 *plan = createFuzzyNumber(25,2,_input,_op);
 _rl = _op.and(_isEqual(_t_pos.mfFU,pos),_isEqual(_t_angle.mfLB,angle));
 plan->degree[_i_plan] = _rl;
 plan->conc[_i_plan] =  _t_plan.mfPB;
 _i_plan++;
 _rl = _op.and(_isEqual(_t_pos.mfFU,pos),_isEqual(_t_angle.mfLE,angle));
 plan->degree[_i_plan] = _rl;
 plan->conc[_i_plan] =  _t_plan.mfPB;
 _i_plan++;
 _rl = _op.and(_isEqual(_t_pos.mfFU,pos),_isEqual(_t_angle.mfCE,angle));
 plan->degree[_i_plan] = _rl;
 plan->conc[_i_plan] =  _t_plan.mfPB;
 _i_plan++;
 _rl = _op.and(_isEqual(_t_pos.mfFU,pos),_isEqual(_t_angle.mfRI,angle));
 plan->degree[_i_plan] = _rl;
 plan->conc[_i_plan] =  _t_plan.mfPB;
 _i_plan++;
 _rl = _op.and(_isEqual(_t_pos.mfFU,pos),_isEqual(_t_angle.mfRB,angle));
 plan->degree[_i_plan] = _rl;
 plan->conc[_i_plan] =  _t_plan.mfPB;
 _i_plan++;
 _rl = _op.and(_isEqual(_t_pos.mfFC,pos),_isEqual(_t_angle.mfLB,angle));
 plan->degree[_i_plan] = _rl;
 plan->conc[_i_plan] =  _t_plan.mfPB;
 _i_plan++;
 _rl = _op.and(_isEqual(_t_pos.mfFC,pos),_isEqual(_t_angle.mfLE,angle));
 plan->degree[_i_plan] = _rl;
 plan->conc[_i_plan] =  _t_plan.mfPB;
 _i_plan++;
 _rl = _op.and(_isEqual(_t_pos.mfFC,pos),_isEqual(_t_angle.mfCE,angle));
 plan->degree[_i_plan] = _rl;
 plan->conc[_i_plan] =  _t_plan.mfBA;
 _i_plan++;
 _rl = _op.and(_isEqual(_t_pos.mfFC,pos),_isEqual(_t_angle.mfRI,angle));
 plan->degree[_i_plan] = _rl;
 plan->conc[_i_plan] =  _t_plan.mfPB;
 _i_plan++;
 _rl = _op.and(_isEqual(_t_pos.mfFC,pos),_isEqual(_t_angle.mfRB,angle));
 plan->degree[_i_plan] = _rl;
 plan->conc[_i_plan] =  _t_plan.mfPB;
 _i_plan++;
 _rl = _op.and(_isEqual(_t_pos.mfNU,pos),_isEqual(_t_angle.mfLB,angle));
 plan->degree[_i_plan] = _rl;
 plan->conc[_i_plan] =  _t_plan.mfBA;
 _i_plan++;
 _rl = _op.and(_isEqual(_t_pos.mfNU,pos),_isEqual(_t_angle.mfLE,angle));
 plan->degree[_i_plan] = _rl;
 plan->conc[_i_plan] =  _t_plan.mfFO;
 _i_plan++;
 _rl = _op.and(_isEqual(_t_pos.mfNU,pos),_isEqual(_t_angle.mfCE,angle));
 plan->degree[_i_plan] = _rl;
 plan->conc[_i_plan] =  _t_plan.mfFO;
 _i_plan++;
 _rl = _op.and(_isEqual(_t_pos.mfNU,pos),_isEqual(_t_angle.mfRI,angle));
 plan->degree[_i_plan] = _rl;
 plan->conc[_i_plan] =  _t_plan.mfFO;
 _i_plan++;
 _rl = _op.and(_isEqual(_t_pos.mfNU,pos),_isEqual(_t_angle.mfRB,angle));
 plan->degree[_i_plan] = _rl;
 plan->conc[_i_plan] =  _t_plan.mfBA;
 _i_plan++;
 _rl = _op.and(_isEqual(_t_pos.mfNC,pos),_isEqual(_t_angle.mfLB,angle));
 plan->degree[_i_plan] = _rl;
 plan->conc[_i_plan] =  _t_plan.mfBA;
 _i_plan++;
 _rl = _op.and(_isEqual(_t_pos.mfNC,pos),_isEqual(_t_angle.mfLE,angle));
 plan->degree[_i_plan] = _rl;
 plan->conc[_i_plan] =  _t_plan.mfFO;
 _i_plan++;
 _rl = _op.and(_isEqual(_t_pos.mfNC,pos),_isEqual(_t_angle.mfCE,angle));
 plan->degree[_i_plan] = _rl;
 plan->conc[_i_plan] =  _t_plan.mfBA;
 _i_plan++;
 _rl = _op.and(_isEqual(_t_pos.mfNC,pos),_isEqual(_t_angle.mfRI,angle));
 plan->degree[_i_plan] = _rl;
 plan->conc[_i_plan] =  _t_plan.mfFO;
 _i_plan++;
 _rl = _op.and(_isEqual(_t_pos.mfNC,pos),_isEqual(_t_angle.mfRB,angle));
 plan->degree[_i_plan] = _rl;
 plan->conc[_i_plan] =  _t_plan.mfBA;
 _i_plan++;
 _rl = _op.and(_isEqual(_t_pos.mfCC,pos),_isEqual(_t_angle.mfLB,angle));
 plan->degree[_i_plan] = _rl;
 plan->conc[_i_plan] =  _t_plan.mfBA;
 _i_plan++;
 _rl = _op.and(_isEqual(_t_pos.mfCC,pos),_isEqual(_t_angle.mfLE,angle));
 plan->degree[_i_plan] = _rl;
 plan->conc[_i_plan] =  _t_plan.mfFO;
 _i_plan++;
 _rl = _op.and(_isEqual(_t_pos.mfCC,pos),_isEqual(_t_angle.mfCE,angle));
 plan->degree[_i_plan] = _rl;
 plan->conc[_i_plan] =  _t_plan.mfBA;
 _i_plan++;
 _rl = _op.and(_isEqual(_t_pos.mfCC,pos),_isEqual(_t_angle.mfRI,angle));
 plan->degree[_i_plan] = _rl;
 plan->conc[_i_plan] =  _t_plan.mfFO;
 _i_plan++;
 _rl = _op.and(_isEqual(_t_pos.mfCC,pos),_isEqual(_t_angle.mfRB,angle));
 plan->degree[_i_plan] = _rl;
 plan->conc[_i_plan] =  _t_plan.mfBA;
 _i_plan++;
 _output = _op.defuz(*plan);
 *plan = createCrispNumber(_output);
}

/***************************************/
/*  Rulebase RL_direction */
/***************************************/

static void RL_direction(FuzzyNumber plan, FuzzyNumber oldway, FuzzyNumber *way) {
 OperatorSet _op = createOP_classifset();
 double _rl, _output;
 int _i_way=0;
 TP_TPlan _t_plan = createTP_TPlan();
 TP_TPlan _t_oldway = createTP_TPlan();
 TP_TWay _t_way = createTP_TWay();
 double _input[2];
 _input[0] = plan.crispvalue;
 _input[1] = oldway.crispvalue;
 *way = createFuzzyNumber(25,2,_input,_op);
 _rl = _op.and(_isEqual(_t_plan.mfPB,plan),_isEqual(_t_oldway.mfPB,oldway));
 way->degree[_i_way] = _rl;
 way->conc[_i_way] =  _t_way.mfBA;
 _i_way++;
 _rl = _op.and(_isEqual(_t_plan.mfPB,plan),_isEqual(_t_oldway.mfBA,oldway));
 way->degree[_i_way] = _rl;
 way->conc[_i_way] =  _t_way.mfBA;
 _i_way++;
 _rl = _op.and(_isEqual(_t_plan.mfPB,plan),_isEqual(_t_oldway.mfST,oldway));
 way->degree[_i_way] = _rl;
 way->conc[_i_way] =  _t_way.mfBA;
 _i_way++;
 _rl = _op.and(_isEqual(_t_plan.mfPB,plan),_isEqual(_t_oldway.mfFO,oldway));
 way->degree[_i_way] = _rl;
 way->conc[_i_way] =  _t_way.mfFO;
 _i_way++;
 _rl = _op.and(_isEqual(_t_plan.mfPB,plan),_isEqual(_t_oldway.mfPR,oldway));
 way->degree[_i_way] = _rl;
 way->conc[_i_way] =  _t_way.mfFO;
 _i_way++;
 _rl = _op.and(_isEqual(_t_plan.mfBA,plan),_isEqual(_t_oldway.mfPB,oldway));
 way->degree[_i_way] = _rl;
 way->conc[_i_way] =  _t_way.mfBA;
 _i_way++;
 _rl = _op.and(_isEqual(_t_plan.mfBA,plan),_isEqual(_t_oldway.mfBA,oldway));
 way->degree[_i_way] = _rl;
 way->conc[_i_way] =  _t_way.mfBA;
 _i_way++;
 _rl = _op.and(_isEqual(_t_plan.mfBA,plan),_isEqual(_t_oldway.mfST,oldway));
 way->degree[_i_way] = _rl;
 way->conc[_i_way] =  _t_way.mfBA;
 _i_way++;
 _rl = _op.and(_isEqual(_t_plan.mfBA,plan),_isEqual(_t_oldway.mfFO,oldway));
 way->degree[_i_way] = _rl;
 way->conc[_i_way] =  _t_way.mfBA;
 _i_way++;
 _rl = _op.and(_isEqual(_t_plan.mfBA,plan),_isEqual(_t_oldway.mfPR,oldway));
 way->degree[_i_way] = _rl;
 way->conc[_i_way] =  _t_way.mfBA;
 _i_way++;
 _rl = _op.and(_isEqual(_t_plan.mfST,plan),_isEqual(_t_oldway.mfPB,oldway));
 way->degree[_i_way] = _rl;
 way->conc[_i_way] =  _t_way.mfST;
 _i_way++;
 _rl = _op.and(_isEqual(_t_plan.mfST,plan),_isEqual(_t_oldway.mfBA,oldway));
 way->degree[_i_way] = _rl;
 way->conc[_i_way] =  _t_way.mfST;
 _i_way++;
 _rl = _op.and(_isEqual(_t_plan.mfST,plan),_isEqual(_t_oldway.mfST,oldway));
 way->degree[_i_way] = _rl;
 way->conc[_i_way] =  _t_way.mfST;
 _i_way++;
 _rl = _op.and(_isEqual(_t_plan.mfST,plan),_isEqual(_t_oldway.mfFO,oldway));
 way->degree[_i_way] = _rl;
 way->conc[_i_way] =  _t_way.mfST;
 _i_way++;
 _rl = _op.and(_isEqual(_t_plan.mfST,plan),_isEqual(_t_oldway.mfPR,oldway));
 way->degree[_i_way] = _rl;
 way->conc[_i_way] =  _t_way.mfST;
 _i_way++;
 _rl = _op.and(_isEqual(_t_plan.mfFO,plan),_isEqual(_t_oldway.mfPB,oldway));
 way->degree[_i_way] = _rl;
 way->conc[_i_way] =  _t_way.mfFO;
 _i_way++;
 _rl = _op.and(_isEqual(_t_plan.mfFO,plan),_isEqual(_t_oldway.mfBA,oldway));
 way->degree[_i_way] = _rl;
 way->conc[_i_way] =  _t_way.mfFO;
 _i_way++;
 _rl = _op.and(_isEqual(_t_plan.mfFO,plan),_isEqual(_t_oldway.mfST,oldway));
 way->degree[_i_way] = _rl;
 way->conc[_i_way] =  _t_way.mfFO;
 _i_way++;
 _rl = _op.and(_isEqual(_t_plan.mfFO,plan),_isEqual(_t_oldway.mfFO,oldway));
 way->degree[_i_way] = _rl;
 way->conc[_i_way] =  _t_way.mfFO;
 _i_way++;
 _rl = _op.and(_isEqual(_t_plan.mfFO,plan),_isEqual(_t_oldway.mfPR,oldway));
 way->degree[_i_way] = _rl;
 way->conc[_i_way] =  _t_way.mfFO;
 _i_way++;
 _rl = _op.and(_isEqual(_t_plan.mfPR,plan),_isEqual(_t_oldway.mfPB,oldway));
 way->degree[_i_way] = _rl;
 way->conc[_i_way] =  _t_way.mfBA;
 _i_way++;
 _rl = _op.and(_isEqual(_t_plan.mfPR,plan),_isEqual(_t_oldway.mfBA,oldway));
 way->degree[_i_way] = _rl;
 way->conc[_i_way] =  _t_way.mfBA;
 _i_way++;
 _rl = _op.and(_isEqual(_t_plan.mfPR,plan),_isEqual(_t_oldway.mfST,oldway));
 way->degree[_i_way] = _rl;
 way->conc[_i_way] =  _t_way.mfST;
 _i_way++;
 _rl = _op.and(_isEqual(_t_plan.mfPR,plan),_isEqual(_t_oldway.mfFO,oldway));
 way->degree[_i_way] = _rl;
 way->conc[_i_way] =  _t_way.mfFO;
 _i_way++;
 _rl = _op.and(_isEqual(_t_plan.mfPR,plan),_isEqual(_t_oldway.mfPR,oldway));
 way->degree[_i_way] = _rl;
 way->conc[_i_way] =  _t_way.mfFO;
 _i_way++;
 _output = _op.defuz(*way);
 *way = createCrispNumber(_output);
}

/***************************************/
/*  Rulebase RL_backward */
/***************************************/

static void RL_backward(FuzzyNumber x, FuzzyNumber angle, FuzzyNumber *wheel) {
 OperatorSet _op = createOP_opset();
 double _rl, _output;
 int _i_wheel=0;
 TP_Tx _t_x = createTP_Tx();
 TP_Tangle _t_angle = createTP_Tangle();
 TP_TWheel _t_wheel = createTP_TWheel();
 double _input[2];
 _input[0] = x.crispvalue;
 _input[1] = angle.crispvalue;
 *wheel = createFuzzyNumber(49,2,_input,_op);
 _rl = _op.and(_isEqual(_t_x.mf0,x),_isEqual(_t_angle.mf0,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf0,x),_isEqual(_t_angle.mf1,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf0,x),_isEqual(_t_angle.mf2,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf0,x),_isEqual(_t_angle.mf3,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf0,x),_isEqual(_t_angle.mf4,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf0,x),_isEqual(_t_angle.mf5,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf0,x),_isEqual(_t_angle.mf6,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf1,x),_isEqual(_t_angle.mf0,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf1,x),_isEqual(_t_angle.mf1,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf1,x),_isEqual(_t_angle.mf2,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf1,x),_isEqual(_t_angle.mf3,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf1,x),_isEqual(_t_angle.mf4,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf1,x),_isEqual(_t_angle.mf5,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf1,x),_isEqual(_t_angle.mf6,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf2,x),_isEqual(_t_angle.mf0,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf2,x),_isEqual(_t_angle.mf1,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf2,x),_isEqual(_t_angle.mf2,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf2,x),_isEqual(_t_angle.mf3,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf2,x),_isEqual(_t_angle.mf4,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf2,x),_isEqual(_t_angle.mf5,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf2,x),_isEqual(_t_angle.mf6,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf3,x),_isEqual(_t_angle.mf0,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf3,x),_isEqual(_t_angle.mf1,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf3,x),_isEqual(_t_angle.mf2,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf3,x),_isEqual(_t_angle.mf3,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf2;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf3,x),_isEqual(_t_angle.mf4,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf3,x),_isEqual(_t_angle.mf5,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf3,x),_isEqual(_t_angle.mf6,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf4,x),_isEqual(_t_angle.mf0,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf4,x),_isEqual(_t_angle.mf1,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf4,x),_isEqual(_t_angle.mf2,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf4,x),_isEqual(_t_angle.mf3,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf4,x),_isEqual(_t_angle.mf4,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf4,x),_isEqual(_t_angle.mf5,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf4,x),_isEqual(_t_angle.mf6,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf5,x),_isEqual(_t_angle.mf0,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf5,x),_isEqual(_t_angle.mf1,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf5,x),_isEqual(_t_angle.mf2,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf5,x),_isEqual(_t_angle.mf3,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf5,x),_isEqual(_t_angle.mf4,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf5,x),_isEqual(_t_angle.mf5,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf5,x),_isEqual(_t_angle.mf6,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf6,x),_isEqual(_t_angle.mf0,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf6,x),_isEqual(_t_angle.mf1,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf6,x),_isEqual(_t_angle.mf2,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf6,x),_isEqual(_t_angle.mf3,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf6,x),_isEqual(_t_angle.mf4,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf6,x),_isEqual(_t_angle.mf5,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf6,x),_isEqual(_t_angle.mf6,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _output = _op.defuz(*wheel);
 *wheel = createCrispNumber(_output);
}

/***************************************/
/*  Rulebase RL_forward */
/***************************************/

static void RL_forward(FuzzyNumber x, FuzzyNumber angle, FuzzyNumber *wheel) {
 OperatorSet _op = createOP_opset();
 double _rl, _output;
 int _i_wheel=0;
 TP_Tx _t_x = createTP_Tx();
 TP_Tangle _t_angle = createTP_Tangle();
 TP_TWheel _t_wheel = createTP_TWheel();
 double _input[2];
 _input[0] = x.crispvalue;
 _input[1] = angle.crispvalue;
 *wheel = createFuzzyNumber(49,2,_input,_op);
 _rl = _op.and(_isEqual(_t_x.mf0,x),_isEqual(_t_angle.mf6,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf0,x),_isEqual(_t_angle.mf5,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf0,x),_isEqual(_t_angle.mf4,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf0,x),_isEqual(_t_angle.mf3,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf0,x),_isEqual(_t_angle.mf2,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf0,x),_isEqual(_t_angle.mf1,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf0,x),_isEqual(_t_angle.mf0,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf1,x),_isEqual(_t_angle.mf6,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf1,x),_isEqual(_t_angle.mf5,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf1,x),_isEqual(_t_angle.mf4,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf1,x),_isEqual(_t_angle.mf3,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf1,x),_isEqual(_t_angle.mf2,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf1,x),_isEqual(_t_angle.mf1,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf1,x),_isEqual(_t_angle.mf0,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf2,x),_isEqual(_t_angle.mf6,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf2,x),_isEqual(_t_angle.mf5,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf2,x),_isEqual(_t_angle.mf4,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf2,x),_isEqual(_t_angle.mf3,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf2,x),_isEqual(_t_angle.mf2,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf2,x),_isEqual(_t_angle.mf1,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf2,x),_isEqual(_t_angle.mf0,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf3,x),_isEqual(_t_angle.mf6,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf3,x),_isEqual(_t_angle.mf5,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf3,x),_isEqual(_t_angle.mf4,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf3,x),_isEqual(_t_angle.mf3,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf2;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf3,x),_isEqual(_t_angle.mf2,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf3,x),_isEqual(_t_angle.mf1,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf3,x),_isEqual(_t_angle.mf0,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf4,x),_isEqual(_t_angle.mf6,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf4,x),_isEqual(_t_angle.mf5,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf4,x),_isEqual(_t_angle.mf4,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf4,x),_isEqual(_t_angle.mf3,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf4,x),_isEqual(_t_angle.mf2,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf4,x),_isEqual(_t_angle.mf1,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf4,x),_isEqual(_t_angle.mf0,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf5,x),_isEqual(_t_angle.mf6,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf5,x),_isEqual(_t_angle.mf5,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf5,x),_isEqual(_t_angle.mf4,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf5,x),_isEqual(_t_angle.mf3,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf5,x),_isEqual(_t_angle.mf2,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf5,x),_isEqual(_t_angle.mf1,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf5,x),_isEqual(_t_angle.mf0,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf6,x),_isEqual(_t_angle.mf6,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf6,x),_isEqual(_t_angle.mf5,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf6,x),_isEqual(_t_angle.mf4,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf6,x),_isEqual(_t_angle.mf3,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf6,x),_isEqual(_t_angle.mf2,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf6,x),_isEqual(_t_angle.mf1,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf6,x),_isEqual(_t_angle.mf0,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _output = _op.defuz(*wheel);
 *wheel = createCrispNumber(_output);
}

/***************************************/
/*  Rulebase RL_celerity */
/***************************************/

static void RL_celerity(FuzzyNumber y, FuzzyNumber gamma, FuzzyNumber *cel) {
 OperatorSet _op = createOP_opset();
 double _rl, _output;
 int _i_cel=0;
 TP_TY _t_y = createTP_TY();
 TP_TGamma _t_gamma = createTP_TGamma();
 TP_TCelerity _t_cel = createTP_TCelerity();
 double _input[2];
 _input[0] = y.crispvalue;
 _input[1] = gamma.crispvalue;
 *cel = createFuzzyNumber(25,2,_input,_op);
 _rl = _op.and(_isEqual(_t_y.mfTG,y),_isEqual(_t_gamma.mfNB,gamma));
 cel->degree[_i_cel] = _rl;
 cel->conc[_i_cel] =  _t_cel.stop;
 _i_cel++;
 _rl = _op.and(_isEqual(_t_y.mfTG,y),_isEqual(_t_gamma.mfNS,gamma));
 cel->degree[_i_cel] = _rl;
 cel->conc[_i_cel] =  _t_cel.stop;
 _i_cel++;
 _rl = _op.and(_isEqual(_t_y.mfTG,y),_isEqual(_t_gamma.mfZE,gamma));
 cel->degree[_i_cel] = _rl;
 cel->conc[_i_cel] =  _t_cel.stop;
 _i_cel++;
 _rl = _op.and(_isEqual(_t_y.mfTG,y),_isEqual(_t_gamma.mfPS,gamma));
 cel->degree[_i_cel] = _rl;
 cel->conc[_i_cel] =  _t_cel.stop;
 _i_cel++;
 _rl = _op.and(_isEqual(_t_y.mfTG,y),_isEqual(_t_gamma.mfPB,gamma));
 cel->degree[_i_cel] = _rl;
 cel->conc[_i_cel] =  _t_cel.stop;
 _i_cel++;
 _rl = _op.and(_isEqual(_t_y.mfCL,y),_isEqual(_t_gamma.mfNB,gamma));
 cel->degree[_i_cel] = _rl;
 cel->conc[_i_cel] =  _t_cel.slow;
 _i_cel++;
 _rl = _op.and(_isEqual(_t_y.mfCL,y),_isEqual(_t_gamma.mfNS,gamma));
 cel->degree[_i_cel] = _rl;
 cel->conc[_i_cel] =  _t_cel.slow;
 _i_cel++;
 _rl = _op.and(_isEqual(_t_y.mfCL,y),_isEqual(_t_gamma.mfZE,gamma));
 cel->degree[_i_cel] = _rl;
 cel->conc[_i_cel] =  _t_cel.slow;
 _i_cel++;
 _rl = _op.and(_isEqual(_t_y.mfCL,y),_isEqual(_t_gamma.mfPS,gamma));
 cel->degree[_i_cel] = _rl;
 cel->conc[_i_cel] =  _t_cel.slow;
 _i_cel++;
 _rl = _op.and(_isEqual(_t_y.mfCL,y),_isEqual(_t_gamma.mfPB,gamma));
 cel->degree[_i_cel] = _rl;
 cel->conc[_i_cel] =  _t_cel.slow;
 _i_cel++;
 _rl = _op.and(_isEqual(_t_y.mfNE,y),_isEqual(_t_gamma.mfNB,gamma));
 cel->degree[_i_cel] = _rl;
 cel->conc[_i_cel] =  _t_cel.slow;
 _i_cel++;
 _rl = _op.and(_isEqual(_t_y.mfNE,y),_isEqual(_t_gamma.mfNS,gamma));
 cel->degree[_i_cel] = _rl;
 cel->conc[_i_cel] =  _t_cel.middle;
 _i_cel++;
 _rl = _op.and(_isEqual(_t_y.mfNE,y),_isEqual(_t_gamma.mfZE,gamma));
 cel->degree[_i_cel] = _rl;
 cel->conc[_i_cel] =  _t_cel.middle;
 _i_cel++;
 _rl = _op.and(_isEqual(_t_y.mfNE,y),_isEqual(_t_gamma.mfPS,gamma));
 cel->degree[_i_cel] = _rl;
 cel->conc[_i_cel] =  _t_cel.middle;
 _i_cel++;
 _rl = _op.and(_isEqual(_t_y.mfNE,y),_isEqual(_t_gamma.mfPB,gamma));
 cel->degree[_i_cel] = _rl;
 cel->conc[_i_cel] =  _t_cel.slow;
 _i_cel++;
 _rl = _op.and(_isEqual(_t_y.mfMI,y),_isEqual(_t_gamma.mfNB,gamma));
 cel->degree[_i_cel] = _rl;
 cel->conc[_i_cel] =  _t_cel.middle;
 _i_cel++;
 _rl = _op.and(_isEqual(_t_y.mfMI,y),_isEqual(_t_gamma.mfNS,gamma));
 cel->degree[_i_cel] = _rl;
 cel->conc[_i_cel] =  _t_cel.quick;
 _i_cel++;
 _rl = _op.and(_isEqual(_t_y.mfMI,y),_isEqual(_t_gamma.mfZE,gamma));
 cel->degree[_i_cel] = _rl;
 cel->conc[_i_cel] =  _t_cel.fast;
 _i_cel++;
 _rl = _op.and(_isEqual(_t_y.mfMI,y),_isEqual(_t_gamma.mfPS,gamma));
 cel->degree[_i_cel] = _rl;
 cel->conc[_i_cel] =  _t_cel.quick;
 _i_cel++;
 _rl = _op.and(_isEqual(_t_y.mfMI,y),_isEqual(_t_gamma.mfPB,gamma));
 cel->degree[_i_cel] = _rl;
 cel->conc[_i_cel] =  _t_cel.middle;
 _i_cel++;
 _rl = _op.and(_isEqual(_t_y.mfFA,y),_isEqual(_t_gamma.mfNB,gamma));
 cel->degree[_i_cel] = _rl;
 cel->conc[_i_cel] =  _t_cel.middle;
 _i_cel++;
 _rl = _op.and(_isEqual(_t_y.mfFA,y),_isEqual(_t_gamma.mfNS,gamma));
 cel->degree[_i_cel] = _rl;
 cel->conc[_i_cel] =  _t_cel.quick;
 _i_cel++;
 _rl = _op.and(_isEqual(_t_y.mfFA,y),_isEqual(_t_gamma.mfZE,gamma));
 cel->degree[_i_cel] = _rl;
 cel->conc[_i_cel] =  _t_cel.fast;
 _i_cel++;
 _rl = _op.and(_isEqual(_t_y.mfFA,y),_isEqual(_t_gamma.mfPS,gamma));
 cel->degree[_i_cel] = _rl;
 cel->conc[_i_cel] =  _t_cel.quick;
 _i_cel++;
 _rl = _op.and(_isEqual(_t_y.mfFA,y),_isEqual(_t_gamma.mfPB,gamma));
 cel->degree[_i_cel] = _rl;
 cel->conc[_i_cel] =  _t_cel.middle;
 _i_cel++;
 _output = _op.defuz(*cel);
 *cel = createCrispNumber(_output);
}

/***************************************/
/*          Inference Engine           */
/***************************************/

void Parking3InferenceEngine(double _d_x, double _d_angle, double _d_y, double _d_gamma, double _d_oldway, double *_d_way, double *_d_v, double *_d_fwd, double *_d_bwd) {
 FuzzyNumber x = createCrispNumber(_d_x);
 FuzzyNumber angle = createCrispNumber(_d_angle);
 FuzzyNumber y = createCrispNumber(_d_y);
 FuzzyNumber gamma = createCrispNumber(_d_gamma);
 FuzzyNumber oldway = createCrispNumber(_d_oldway);
 FuzzyNumber way, v, fwd, bwd, pos, plan;
 RL_position(x, y, &pos);
 RL_planning(pos, angle, &plan);
 RL_direction(plan, oldway, &way);
 RL_celerity(y, gamma, &v);
 RL_forward(x, angle, &fwd);
 RL_backward(x, angle, &bwd);
 *_d_way = way.crispvalue;
 *_d_v = v.crispvalue;
 *_d_fwd = fwd.crispvalue;
 *_d_bwd = bwd.crispvalue;
}

